# Untitled - By: imrobot - 周一 2月 6 2023

from machine import SPI
from machine import I2C
from fpioa_manager import fm
from Maix import GPIO
import time
import utime
import sensor, image
import struct
from machine import WDT
from machine import Timer


############################################数据定义#################################################
###颜色阈值##
red_threshold  = (40, 71, 20, 54, 8, 41)
green_threshold = (31, 54, -5, 16, -9, 24)
#PID
Ball_P = 0.00511  #小球跟踪Kp值
Ball_Effect = 10000 #识别小球像素最小值
Line_P = 0.00511

class robot:
    def __init__(self):
        self.Mode = 0x00
        self.Action_sta = 0x00
        self.Action_id = 0x00
        self.X_speed = 0.00
        self.Y_speed = 0.00
        self.Z_speed = 0.00
        self.X_angle = 0.00
        self.Y_angle = 0.00
        self.Z_angle = 0.00
        self.High = 0x00
        self.SpiData_R = bytearray(5)
        #self.data_read = bytearray(1024)
############################################SPI通信#################################################
###SPI等待握手###
def Delay_H():
    global SPI_data_len,cs,RedPIn,spi1
    i=0
    while(1):
        cs.value(0)
        if RedPIn.value() == 1:
            return 1
        else:           #长时间失败退出
            i = i+1
            if i == 2000:
                return 0
###spi等待握手###
def Delay_L():
    global SPI_data_len,cs,RedPIn,spi1
    i=0
    while(1):
        cs.value(1)
        if RedPIn.value() == 0:
            return 1
        else:       #长时间失败退出
            i = i+1
            if i == 2000:
                return 0
###spi发送字节###
def spi_send_r(data,lens):
    global SPI_data_len,cs,RedPIn,spi1,ESP32,Robot
    if  len(data) == 0:
        return
    data_read = bytearray(lens)
    cs.value(0)
    if Delay_H():
        spi1.write_readinto(data,data_read)
    else:
        return
    cs.value(1)
    Delay_L()
    return data_read
###spi发送字节-（无接收数据）###
def spi_send(data):
    global SPI_data_len,mosi,miso,cs,clk,RedPIn,spi1,ESP32
    fm.register(mosi,fm.fpioa.SPI1_D0)#mosi
    fm.register(miso,fm.fpioa.SPI1_D1)#miso
    fm.register(clk,fm.fpioa.SPI1_SCLK)#sclk
    if  len(data) == 0:
        return
    cs.value(0)
    if Delay_H():
        spi1.write(data)
    else:
        return
    cs.value(1)
    Delay_L()

#引脚初始化
def Init():
    global mosi,miso,cs,clk,signal,RedPIn,spi1,led,ESP32,Robot
    Robot = robot()
    mosi=23 #4 23
    miso=22 #3 22
    cs=24   #5 24
    clk=21  #2 21
    signal = 9 #7 15
    #引脚映射
    fm.register(cs,fm.fpioa.GPIOHS10)#cs
    cs = GPIO(GPIO.GPIOHS10, GPIO.OUT)
    #握手信号引脚映射
    fm.register(signal,fm.fpioa.GPIOHS9)#11
    RedPIn = GPIO(GPIO.GPIOHS9,GPIO.IN)
    #其他引脚定义
    #fm.register(12,fm.fpioa.GPIO0)
    #led=GPIO(GPIO.GPIO0,GPIO.OUT)
    fm.register(8,fm.fpioa.GPIO2)
    ESP32=GPIO(GPIO.GPIO2,GPIO.IN,GPIO.PULL_DOWN )

    while(1):
        print("等待esp32开启")
        time.sleep(1)
        if ESP32.value() == 1:
            print("开启spi初始化")
            break
    #开启SPI
    fm.register(mosi,fm.fpioa.SPI1_D0)#mosi
    fm.register(miso,fm.fpioa.SPI1_D1)#miso
    fm.register(clk,fm.fpioa.SPI1_SCLK)#sclk
    spi1 = SPI(SPI.SPI1, mode=SPI.MODE_MASTER, baudrate=4000000, polarity=0, phase=0, bits=8, firstbit=SPI.MSB)#10000000
    cs.value(1)
    time.sleep(1)
    #发送空数值
    data1 = bytearray(8)
    data1[0] = 0xAA
    data1[1] = 0xAF
    spi_send(data1)
    spi_send(data1)
############################################基础控制#################################################
###float转4字节十六进制###
def send_float(tx_Buf,data):
    temp_B= struct.pack('f',float(data))
    tx_Buf.append(temp_B[0])
    tx_Buf.append(temp_B[1])
    tx_Buf.append(temp_B[2])
    tx_Buf.append(temp_B[3])
###int转4字节十六进制###
def send_int(tx_Buf,data):
    temp_B= struct.pack('i',int(data))
    tx_Buf.append(temp_B[0])
    tx_Buf.append(temp_B[1])
    tx_Buf.append(temp_B[2])
    tx_Buf.append(temp_B[3])
###char转4字节十六进制###
def send_char(tx_Buf,data):
    tx_Buf.append(int(data))

def sport_control_send():
    global Mode,Action_sta,Action_id,X_speed,Y_speed,Z_speed ,X_angle,Y_angle,Z_angle,High,tx_data,Robot
    tx_data =  bytearray(1024)
    sum = 0x00
    data_to_send  =  []
    data_to_send.append(0xAA)
    data_to_send.append(0xAF)
    send_char(data_to_send,Robot.Mode)#Mode
    send_char(data_to_send,Robot.Action_sta)#Action_sta
    send_char(data_to_send,Robot.Action_id)#Action_id
    send_float(data_to_send,Robot.X_speed)#Robot_X_speed
    send_float(data_to_send,Robot.Y_speed)#Robot_Y_speed
    send_float(data_to_send,Robot.Z_speed)
    send_float(data_to_send,Robot.X_angle)
    send_float(data_to_send,Robot.Y_angle)
    send_float(data_to_send,Robot.Z_angle)
    send_char(data_to_send,Robot.High)#High
    for i in range(0,len(data_to_send)):
        sum = sum +  data_to_send[i]
    data_to_send.append(sum)
    data_to_send.append(0x00)
    data_to_send.append(0x00)
    data_to_send.append(0x00)
    for i in range(0,len(data_to_send)):
        tx_data[i] = data_to_send[i]
    tx_data[1022] = 0x00
    tx_data[1023] =  len(data_to_send)
    spi_send(tx_data)

#模式
def Robots_mode(mode):
    global Robot
    Robot.Mode = mode
    sport_control_send()
    Robot.Mode = 0x00

#动作组
def Robots_Action(sta,ids):
    global Robot
    Robot.Action_sta = sta
    Robot.Action_id = ids
    sport_control_send()

#前进速度（-1.4~1.4）
def Robots_Xspeed(speed):
    global Robot
    Robot.X_speed = speed
    sport_control_send()

#左右移动速度（-1.4~1.4）
def Robots_Yspeed(speed):
    global Robot
    Robot.Y_speed = speed
    sport_control_send()

#右转转动速度（-1.4~1.4）
def Robots_Zspeed(speed):
    global Robot
    Robot.Z_speed = speed
    #print(Z_speed)
    sport_control_send()

#俯仰角度（-1.4~1.4）
def Robots_Xangle(angle):
    global Robot
    Robot.X_angle = angle
    sport_control_send()

#左右倾斜角度（-1.4~1.4）
def Robots_Yangle(angle):
    global Robot
    Robot.Y_angle = angle
    sport_control_send()

#左右扭动角度（-1.4~1.4）
def Robots_Zangle(angle):
    global Robot
    Robot.Z_angle = angle
    sport_control_send()

#高度（0~10）
def Robots_High(high):
    global Robot
    Robot.High = high
    sport_control_send()

#复位
def Robot_Reset():
    Robots_Xspeed(0)
    Robots_Yspeed(0)
    Robots_Zspeed(0)
    Robots_Xangle(0)
    Robots_Yangle(0)
    Robots_Zangle(0)

#接收数据-机器人状态、k210控制功能数据（机器人状态、动作组状态、摄像头控制1、功能控制2）
def Robots_Sta():
    global Robot
    #print(1)
    data_send = bytearray([0xCA,0xCF,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00])
    data_read = spi_send_r(data_send,len(data_send))
    if data_read  == None:
        return
    #print(data_read)
    sums = 0
    lens = 0
    if(data_read[0] == 0xBA and data_read[1] == 0xBF ):
        lens = data_read[2] + 2
        for i in range(0,lens+1):
            sums += data_read[i]
        sums = sums&0xff #取低位
        if(sums == data_read[lens+1]):
            return data_read[3:lens+1]
    for i in range(3,8):
        data_read[i] = 0
    return data_read[3:8]
############################################手势识别#################################################
PAJ7620_ID = 0x73                   #从机ID
PAJ7620_REGITER_BANK_SEL = 0xEF     #选择寄存器带区
PAJ7620_BANK0 = 0x00                #带区0
PAJ7620_BANK1 = 0x01                #带区1

ISENABLE = 0x72                     #使能/失能寄存器
GES_DETECT = 0x43                   #读取手势（1-8）
GES_DETECT2 = 0x44                  #读取手势（9）
SUSPEND = 0x03                      #使模块挂起（休眠）

#手势识别方式
left = 1                            #从右到左
right = 2                           #从左到右
down = 4                            #从上往下
up = 8                              #从下往上
forward = 16                        #从后往前
backward = 32                       #从前往后
clockwise = 64                      #顺时针
count_clockwise = 128               #逆时针
wave = 1                            #挥手

Init_Register_Array = [
    [0xEF, 0x00],
    [0x37, 0x07],
    [0x38, 0x17],
    [0x39, 0x06],
    [0x41, 0x00],
    [0x42, 0x00],
    [0x46, 0x2D],
    [0x47, 0x0F],
    [0x48, 0x3C],
    [0x49, 0x00],
    [0x4A, 0x1E],
    [0x4C, 0x20],
    [0x51, 0x10],
    [0x5E, 0x10],
    [0x60, 0x27],
    [0x80, 0x42],
    [0x81, 0x44],
    [0x82, 0x04],
    [0x8B, 0x01],
    [0x90, 0x06],
    [0x95, 0x0A],
    [0x96, 0x0C],
    [0x97, 0x05],
    [0x9A, 0x14],
    [0x9C, 0x3F],
    [0xA5, 0x19],
    [0xCC, 0x19],
    [0xCD, 0x0B],
    [0xCE, 0x13],
    [0xCF, 0x64],
    [0xD0, 0x21],
    [0xEF, 0x01],
    [0x02, 0x0F],
    [0x03, 0x10],
    [0x04, 0x02],
    [0x25, 0x01],
    [0x27, 0x39],
    [0x28, 0x7F],
    [0x29, 0x08],
    [0x3E, 0xFF],
    [0x5E, 0x3D],
    [0x65, 0x96],
    [0x67, 0x97],
    [0x69, 0xCD],
    [0x6A, 0x01],
    [0x6D, 0x2C],
    [0x6E, 0x01],
    [0x72, 0x01],
    [0x73, 0x35],
    [0x74, 0x00],
    [0x77, 0x01]]

Init_PS_Array = [
    [0xEF, 0x00],
    [0x41, 0x00],
    [0x42, 0x00],
    [0x48, 0x3C],
    [0x49, 0x00],
    [0x51, 0x13],
    [0x83, 0x20],
    [0x84, 0x20],
    [0x85, 0x00],
    [0x86, 0x10],
    [0x87, 0x00],
    [0x88, 0x05],
    [0x89, 0x18],
    [0x8A, 0x10],
    [0x9f, 0xf8],
    [0x69, 0x96],
    [0x6A, 0x02],
    [0xEF, 0x01],
    [0x01, 0x1E],
    [0x02, 0x0F],
    [0x03, 0x10],
    [0x04, 0x02],
    [0x41, 0x50],
    [0x43, 0x34],
    [0x65, 0xCE],
    [0x66, 0x0B],
    [0x67, 0xCE],
    [0x68, 0x0B],
    [0x69, 0xE9],
    [0x6A, 0x05],
    [0x6B, 0x50],
    [0x6C, 0xC3],
    [0x6D, 0x50],
    [0x6E, 0xC3],
    [0x74, 0x05]]

Init_Gesture_Array = [
    [0xEF, 0x00],
    [0x41, 0x00],
    [0x42, 0x00],
    [0xEF, 0x00],
    [0x48, 0x3C],
    [0x49, 0x00],
    [0x51, 0x10],
    [0x83, 0x20],
    [0x9F, 0xF9],
    [0xEF, 0x01],
    [0x01, 0x1E],
    [0x02, 0x0F],
    [0x03, 0x10],
    [0x04, 0x02],
    [0x41, 0x40],
    [0x43, 0x30],
    [0x65, 0x96],
    [0x66, 0x00],
    [0x67, 0x97],
    [0x68, 0x01],
    [0x69, 0xCD],
    [0x6A, 0x01],
    [0x6B, 0xB0],
    [0x6C, 0x04],
    [0x6D, 0x2C],
    [0x6E, 0x01],
    [0x74, 0x00],
    [0xEF, 0x00],
    [0x41, 0xFF],
    [0x42, 0x01]]


#i2c初始化
i2c = I2C(I2C.I2C5, mode=I2C.MODE_MASTER, scl=30,sda=31,freq=100000, addr_size=7)
#选择寄存器带区
def GestureSelectBank(bank):
    if bank == 0 :
        i2c.writeto_mem(PAJ7620_ID, PAJ7620_REGITER_BANK_SEL, PAJ7620_BANK0, mem_size=8)#写数据到从机寄存器
    elif bank == 1 :
        i2c.writeto_mem(PAJ7620_ID, PAJ7620_REGITER_BANK_SEL, PAJ7620_BANK1, mem_size=8)#写数据到从机寄存器
    else :
        return 255


#发送带区地址和数据
def GestureWriteReg(addr, cmd):
    write_buf = bytearray(2)
    write_buf[0] = addr
    write_buf[1] = cmd
    i2c.writeto_mem(PAJ7620_ID, write_buf[0], write_buf[1], mem_size=8)

#读取手势
def GestureReadReg(addr):
    read_buf = bytearray(1)
    read_buf[0] = addr
    i2c.writeto(PAJ7620_ID, read_buf)

    result = bytearray(1)
    i2c.readfrom_mem_into(PAJ7620_ID, read_buf[0], result, mem_size=8)
    return result

#唤醒和发送数组
def GestureInit():
    #utime.sleep_ms(500)
    for i in range(0,5):
       wakeup = GestureReadReg(0x00)
       utime.sleep_ms(1)
    GestureSelectBank(0)
    result = bytearray(1)
    i2c.readfrom_mem_into(PAJ7620_ID, 0x00, result, mem_size=8)
    #print('result=',result[0])
    if result[0] != 0x20:
        return 0xff
    for i in range(0,len(Init_Register_Array)):
        GestureWriteReg(Init_Register_Array[i][0], Init_Register_Array[i][1])
    for i in range(0,len(Init_Gesture_Array)):
        GestureWriteReg(Init_Gesture_Array[i][0], Init_Gesture_Array[i][1])
    GestureSelectBank(1)#带区1
    GestureWriteReg(ISENABLE, 0x01)#使能
    return 1

#获取手势
def GetGesture():
    GestureSelectBank(0)
    date = GestureReadReg(0x43)
    if date != right or date != left or date != up or date != down  or date != forward or date != backward or date != clockwise or date != count:
        date == GestureReadReg(0x44)
        if date == wave:
            return 256
        return date[0]


#选择手势
def Select_gesture_as(state):
        return state
############################################超声波#################################################
redata = bytearray(3)#返回数据
i2c5 = I2C(I2C.I2C5, mode=I2C.MODE_MASTER, scl=30,sda=31,freq=100000, addr_size=7)
uladdr = 0x57

def ultrasonic_init(uladdr):
    i2c5.writeto(uladdr, '\x01')
    utime.sleep_ms(100)#等待100ms
    i2c5.readfrom_into(0x57, redata)
    #print(redata)
    x = ((redata[0]<<16) + (redata[1]<<8) + redata[2])/1000
    print("x=",x)
    return x


#i2c初始化,获取总线设备
def i2cscan_ultrasonic():
    i2c2 = I2C(I2C.I2C5, mode=I2C.MODE_MASTER, scl=30,sda=31,freq=100000, addr_size=7)#400000
    devices = i2c2.scan()
    #print(devices)
    if devices:
        for device in devices:
            ultrasonic_init(device)
            #print(ultrasonic_init(device))




def i2cscan_gesture():
    i2c2 = I2C(I2C.I2C5, mode=I2C.MODE_MASTER, scl=30,sda=31,freq=100000, addr_size=7)#400000
    devices = i2c2.scan()
    GestureInit()
    if GestureInit() == 1:
        return 1
    # if GestureInit() == 1:
    #         dd = GetGesture()
    #         #print("dd=",dd)
    #         return dd

#######测试
#while (1):
    #i2cscan_ultrasonic()
    #utime.sleep_ms(600)
    #i2cscan_gesture()
    #utime.sleep_ms(500)
    #ultrasonic_init(uladdr)
############################################图传#################################################
#图像数据封装
def spi_send_process(data,sums):
    length = len(data)
    if length == 0 or length > 2048:
        return
    data_len = length.to_bytes(2,'big')
    #print(data_len)
    img_tail = bytearray([0x00,0x00,0x00]) #添加帧尾，防止尾部数据丢失
    img_header = bytearray([0xBA,0xBF,sums])
    img_bytes = b''
    img_bytes = img_bytes + img_header + data_len + data + img_tail
    spi_send(img_bytes)
#图传
img_sum = 0x00
def img_app(imgs,size):
    global img_sum
    imgs = imgs.compress(quality=size)#压缩图片提升传输速率，想要图像更清晰将quality调大一些（quality范围在0~100）
    img_byte = imgs.to_bytes(1,'little') #将图片转成字节数组
    block = int(len(img_byte)/2040)
    for i in range(block):
        spi_send_process(img_byte[i*2040:(i+1)*2040],img_sum) #img_sum
        img_sum = img_sum + 0x01
    spi_send_process(img_byte[block*2040:],0xFF) #img_sum
    img_sum = 0x00

############################################功能#################################################
def img_init():
    sensor.binocular_reset()
    sensor.shutdown(0)  # 选中sensor 0
    sensor.set_pixformat(sensor.RGB565)
    sensor.set_framesize(sensor.QVGA)
    sensor.set_vflip(1)

    sensor.shutdown(1)  # 选中sensor 1
    sensor.set_pixformat(sensor.RGB565)
    sensor.set_framesize(sensor.QVGA)
    sensor.run(1)

#小球识别
def IdentifyLine(img):
    pixels= 200 #识别小球最小像素
    blobs = img.find_blobs([red_threshold],x_stride=15,y_stride=15)
    if blobs:
        for b in blobs:
            if b.pixels() > pixels :
                img.draw_rectangle(b[0:4])
                img.draw_cross(b[5], b[6])
                x = 160 - b.cx()
                Ball_data = []
                Ball_data.append(x)
                Ball_data.append(b.pixels())
                return Ball_data
            return None
    return None

#小球跟踪
def Ball_Tracking(X):
    Robots_High(10)
    if X == None:
        Robots_Xspeed(0)
        Robots_Zspeed(0)
        Robots_mode(7)
        return
    Robots_mode(3) #进入小跑
    if  X[1] < Ball_Effect:
        Robots_Xspeed(0.6)
    else:
        Robots_Xspeed(0)
    xx = -(X[0]*Ball_P)
    Robots_Zspeed(xx)
    #print(X[0]*Ball_P)

#寻找最大色块函数定义
def find_max(blobs):
    if blobs:
        max_size=0
        for blob in blobs:
            if blob[4] > max_size:
                max_blob=blob
                max_size = blob[4]
        return max_blob

#巡线
def Line_distinguish(img):
    xx = 0
    yy = 0
    x = 0
    y = 0
    x1 = 0
    y1 = 0
    s = 0
    for i in range(0,5):
        ii = i*24
        blobs = img.find_blobs([green_threshold], roi=[0,ii,320,ii+24],x_stride = 10,pixels_threshold = 10,merge_cb=None)#merge_cb=True
        if blobs:
            max_blob = find_max(blobs)
            xx = xx + max_blob[5]
            yy = yy + max_blob[6]
            s = s+1
    if s > 1:
        x = int(xx /s)
        y = int(yy /s)
        img.draw_cross(x, y)
    s =0
    xx = 0
    yy = 0
    for i in range(5,10):
        ii = i*24
        blobs = img.find_blobs([green_threshold], roi=[0,ii,320,ii+24],x_stride = 10,pixels_threshold = 10,merge_cb=None)#merge_cb=True
        if blobs:
            max_blob = find_max(blobs)
            xx = xx + max_blob[5]
            yy = yy + max_blob[6]
            s = s+1
    if s > 1:
        x1 = int(xx /s)
        y1 = int(yy /s)
        img.draw_cross(x1, y1)
    if x and x1:
        img.draw_line((x,y,x1,y1), color = 150)
        x = (x+x1)/2
        x = 160 - x
        return x
    return None

def Line_patrol(X):
    Robots_High(10)
    if X == None:
        Robots_Xspeed(0)
        Robots_Zspeed(0)
        Robots_mode(7)
        Robots_High(10)
        return
    if X != None:
        utime.sleep_ms(1000)
        Robots_mode(3) #进入小跑
        Robots_Xspeed(0.6) #前进
        X = -(X*Line_P)
        Robots_Zspeed(X) #左右转






############################################其他#################################################
####喂狗回调函数###
#def on_wdt(self):
    #print(self.context(), self)
    #self.feed()
####定时器回调函数###
#def on_timer(timer):
    #global led_sta,Robot,SpiData_R
    ##LED灯闪烁
    #led_sta = ~led_sta
    #led.value(led_sta)
    ##心跳
    ##print("定时器心跳数据发送")
    #SpiData_R = Robots_Sta()
    #print(SpiData_R)

def camera_init():
  sensor.binocular_reset()
  sensor.set_pixformat(sensor.RGB565)
  sensor.set_framesize(sensor.QVGA)
  sensor.skip_frames(10)
  sensor.run(1)


Init()
Robots_mode(0x02)
time.sleep_ms(100);
camera_init()
sensor.shutdown(0)
sensor.set_vflip(1)
while True:
  img = sensor.snapshot()
  Line_patrol(Line_distinguish(img))



